/**
 * Author: luoqi
 * Created Date: 2025-11-20 00:21:43
 * Last Modified: 2025-11-20 00:47:50
 * Modified By: luoqi at <**@****>
 * Copyright (c) 2025 <*****>
 * Description:
 */

#include <bsp.h>
#include <cli.h>
#include "joy.h"
#include "ctrl.h"

static float joy_pos = 0.0f;
static int joy_dir = 1;

static void joy_task()
{
    joy_pos += 6.0f * joy_dir;
    if(joy_pos > 360.0f) {
        joy_pos -= 360.0f;
    } else if(joy_pos < 0.0f) {
        joy_pos += 360.0f;
    }
    
    ctrl_pset(joy_pos);
}

static int cmd_joy_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" [<off/on>], start or stop joy task\r\n");
        QSH(" [<cw/ccw>], set joy direction\r\n");
        return 0;
    }
    
    if(ISARG(argv[1], "on")) {
        if(ctrl_mod_get() != MC_MOD_POS) {
            QSH(" ctrl mod must be position mode\r\n");
            return 0;
        }
        joy_pos = ctrl_pos_get();
        task_resume("joy");
    } else if(ISARG(argv[1], "off")) {
        task_suspend("joy");
    } else if(ISARG(argv[1], "cw")) {
        joy_dir = 1;
    } else if(ISARG(argv[1], "ccw")) {
        joy_dir = -1;
    } else {
        return -1;
    }
    return 0;
}

int joy_init()
{
    task_add("joy", joy_task, 1000);
    task_suspend("joy");

    cli_add("joy", cmd_joy_hdl, "joy funny");
    return 0;
}

